//
//  utils.cpp
//  cali
//
//  Created by Fan Qu on 8/11/19.
//  Copyright © 2019 GF. All rights reserved.
//

#include "utils.h"
#include <cmath>
#include <vector>


double getDis2f(const Pos2F& pos1, const Pos2F& pos2)
{
    double deltaX = pos1.x - pos2.x, deltaY = pos1.y - pos2.y;
    return sqrt(deltaX * deltaX + deltaY * deltaY);
}

Pos2F removeOffset(const Pos2F& pos)
{
    return Pos2F(pos.x + 1.59, pos.y - 12.75);
}

int getTapPosIndex(Pos2F cPoses[], const Pos2F& pos)
{
    double minDis = 9999.0f;
    int index = -1;
#ifdef USE_SPACE
    for(int i = 0; i < 28; ++i) {
#else
    for(int i = 0; i < 26; ++i) {
#endif
        Pos2F tempPos = Pos2F(cPoses[i].x, cPoses[i].y);
        double dis = getDis2f(removeOffset(pos), tempPos);
        if(dis < minDis) {
            minDis = dis;
            index = i;
        }
    }
    return index;
}

